#!/usr/bin/python3
# coding=utf8
import sys
import time
import threading
import HiwonderSDK.Board as Board
import Functions.Avoidance_obstacle as Avoidance
import Functions.VisualPatrol as Tracing


if sys.version_info.major == 2:
    print('Please run this program with python3!')
    sys.exit(0)


# 线程锁
trace_lock = threading.Lock()
avoid_lock = threading.Lock()

obstacle = 0  # 是否探测到障碍，未探测到为0，探测到为1


def setBuzzer(timer):
    """
    使蜂鸣器鸣响2声。
    Args:
        timer: 每次鸣响持续时间、两次鸣响之间间隔时间
    Returns:
        None: 
    """
    Board.setBuzzer(0)
    Board.setBuzzer(1)
    time.sleep(timer)
    Board.setBuzzer(0)
    time.sleep(timer)
    Board.setBuzzer(1)
    time.sleep(timer)
    Board.setBuzzer(0)


def obstacleDetection():
    """
    障碍检测线程。
    """
    global obstacle
    try:
        while True:
            with avoid_lock:
                obstacle = Avoidance.detect_obstacle()
                time.sleep(0.05)  # 每隔0.05s检测一次障碍

    except Exception as e:
        print('E01_ObstacleError', e)
        Avoidance.exit()


def lineTracing():
    """
    循迹线程。
    """
    global obstacle
    try:
        with trace_lock:
            Tracing.patrol_run(obstacle)

    except Exception as e:
        print('E02_PatrolError', e)
        Tracing.exit()


def stopCar():
    """
    停止所有模块并退出。
    """
    Avoidance.exit()
    Tracing.exit()


def startCar():
    """
    主控线程负责决策。
    """
    global obstacle
    setBuzzer(0.3)  # 蜂鸣器短响2声开启程序
    print('Main Thread Started')
    while True:
        try:
            with avoid_lock:
                if obstacle:
                    print('Patrol is stopped by Avoidance.')
                    Avoidance.avoidance_run()
                    obstacle = 0

        except KeyboardInterrupt:
            setBuzzer(0.5)  # 蜂鸣器长响2声退出程序
            print("Stopped by Ctrl+C")
            stopCar()
            break
    sys.exit(0)


if __name__ == '__main__':
    # 子线程
    OD = threading.Thread(target=obstacleDetection, daemon=True)
    LT = threading.Thread(target=lineTracing, daemon=True)

    OD.start()
    LT.start()

    startCar()
